/** Calibration abstraction.
 *
 * This file contains calibration parameters and functions to calibrate the ITG
 * and ADXL sensors.
*/
//##############################################################################




/** itg and adxl abstraction layers */
#include "itg.h"
#include "adxl.h"
//------------------------------------------------------------------------------




/** for the timer */
#include "LR_timer.h"
//##############################################################################




/** Default number of calibration samples to take */
#define DEFAULT_SAMPLE_NUM 100.0
//------------------------------------------------------------------------------

//TODO scale factors


/** Scale factors */
// 1 / (14.375 / (PI/180))
#define ITG_SCALE_X .001214142086956521739130434782608695652173913043478260
#define ITG_SCALE_Y .001214142086956521739130434782608695652173913043478260
#define ITG_SCALE_Z .001214142086956521739130434782608695652173913043478260
#define ADXL_SCALE_X .004
#define ADXL_SCALE_Y .004
#define ADXL_SCALE_Z .004
//##############################################################################




/** Slot to run calibration on */
volatile unsigned char slot;
//------------------------------------------------------------------------------




/** Remaining calibration samples */
volatile unsigned int remaining_samples;
//------------------------------------------------------------------------------




/** Vector to store calibration (offset) itg values */
volatile double calib_itg [3];
//------------------------------------------------------------------------------




/** Vector to store calibration (offset) adxl values */
volatile double calib_adxl [3];
//------------------------------------------------------------------------------




/** Vector to store scale factors for itg */
volatile double scale_itg [3];
//------------------------------------------------------------------------------




/** Vector to store scale factors for adxl */
volatile double scale_adxl [3];
//------------------------------------------------------------------------------




/** Temporary storage while calibration is running */
volatile int temp_itg [3];
volatile int temp_adxl [3];
//##############################################################################




/** Sensor calibration timer function.
 *
 * This function gets called by the timer on calibration of the sensors.
*/
__attribute__((__interrupt__))
static void calib_sensors( void )
{
  // If we have remaining calibration samples to take
  if (remaining_samples > 0)
  {
    short ix, iy, iz;
    short ax, ay, az;
    // Read current sensor values
    itg_read( &ix, &iy, &iz );
    adxl_read( &ax, &ay, &az );
    // Add them up such that their mean can be computed later
    temp_itg[0] += ix;
    temp_itg[1] += iy;
    temp_itg[2] += iz;
    temp_adxl[0] += ax;
    temp_adxl[1] += ay;
    temp_adxl[2] += az;
    // One sample less to take
    remaining_samples--;
  }
  // No remaining samples to take
  else
  {
    // Stop the timer
    LR_timer_stop( slot );
  }
  // Allow new timer interrupt
  LR_timer_sti( slot );
}
//------------------------------------------------------------------------------


#include "compiler.h"

/** Calibration function.
 *
 * This function registers a calibration function as a timer, and computes the
 * mean of the sensor values over time. Those values are stored inside the
 * calibration (offset) variables. The scale factors are predefined.
*/
void calibrate( unsigned char s )
{
  slot = s;
  // Set the number of samples to be taken
  remaining_samples = DEFAULT_SAMPLE_NUM;
  // Set calibration parameter to initial zero
  temp_itg[0] = 0;
  temp_itg[1] = 0;
  temp_itg[2] = 0;
  temp_adxl[0] = 0;
  temp_adxl[1] = 0;
  temp_adxl[2] = 0;
  // Assign the calibration function to a timer
  LR_timer_assign( slot , calib_sensors, 5 );
  LR_timer_start( slot );
  // Wait until calibration finishes
  LR_timer_wait( slot );
  // Compute mean calibration values
  calib_itg[0] = temp_itg[0] / DEFAULT_SAMPLE_NUM;
  calib_itg[1] = temp_itg[1] / DEFAULT_SAMPLE_NUM;
  calib_itg[2] = temp_itg[2] / DEFAULT_SAMPLE_NUM;
  calib_adxl[0] = temp_adxl[0] / DEFAULT_SAMPLE_NUM;
  calib_adxl[1] = temp_adxl[1] / DEFAULT_SAMPLE_NUM;
  calib_adxl[2] = temp_adxl[2] / DEFAULT_SAMPLE_NUM;
  // Set scale factors to their default values
  scale_itg[0] = ITG_SCALE_X;
  scale_itg[1] = ITG_SCALE_Y;
  scale_itg[2] = ITG_SCALE_Z;
  scale_adxl[0] = ADXL_SCALE_X;
  scale_adxl[1] = ADXL_SCALE_Y;
  scale_adxl[2] = ADXL_SCALE_Z;
}
//##############################################################################




